#include "linearvertexcluster.h"

#include "DiagMatrix.h"
#include "consoleoutput.h"

LinearVertexCluster::LinearVertexCluster( DeformableMesh& mesh, int clusterId ) : 
	VertexCluster( mesh, clusterId )
{
}


LinearVertexCluster::~LinearVertexCluster(void)
{
}

void LinearVertexCluster::updateInternal( float dt )
{
	float rawMatR[16];
	matR.GetMatrix( rawMatR );

	// calculating the goal position matrix,
	// which is a composition of the R matrix with the A matrix
	VMatrix goalPositionMatrix;
	goalPositionMatrix.LoadMatrix( rawMatR );

	float meshBeta = getMeshBeta();
	goalPositionMatrix.ScalarMult3x3( 1.0f - meshBeta );

	// multiplying by the beta factor and summing
	matA.ScalarMult3x3( meshBeta );
	goalPositionMatrix.Sum( matA );
	matA.ScalarMult3x3( 1.0f / meshBeta );

	if( recalculateWeights )
		resetWeights();

	for( int i = 0; i < numberOfVertices; i++ )
	{
		float relativeGi[4];
		goalPositionMatrix.MultVector( &q[i * 3], relativeGi );

		updateSpeedAndPosition( relativeGi, i, dt );
	}
}

void LinearVertexCluster::updateQVector( LAMatrix& Sp )
{
	// updating qi and Aqq
	for( int i = 0; i < numberOfVertices; i++ )
	{
		float qVector[4] = {
			originalVertexBuffer[ i*3 + 0 ] - cm0[ 0 ],
			originalVertexBuffer[ i*3 + 1 ] - cm0[ 1 ],
			originalVertexBuffer[ i*3 + 2 ] - cm0[ 2 ],
			0.0f
		};
		
		float spQ[4] = { 0.0f, 0.0f, 0.0f, 0.0f };

		Sp.mult( qVector, spQ );
		q[ i*3 + 0 ] = spQ[ 0 ];
		q[ i*3 + 1 ] = spQ[ 1 ];
		q[ i*3 + 2 ] = spQ[ 2 ];
	}
}

